#ifndef __EM_PLANNER_OBSTACLE_HPP__
#define __EM_PLANNER_OBSTACLE_HPP__

#include <iostream>
#include <vector>
#include <cmath>
#include <Eigen/Dense>

#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <visualization_msgs/Marker.h>


#include "./em_utils.hpp"

class GenObstacle
{
private:
    
    ros::NodeHandle nh;
    ros::Subscriber global_path_sub;
    ros::Publisher obs_pub;

    double noise;                         // 障碍物位置扰动
    double obs_scale;                     // 障碍物大小
    vector<int> obs_index;                      // 障碍位置在全局路径的下标
    vector<Location> obstacle_vec;              // 障碍物坐标信息

    double obs_speed = 0.15;                     // 动态障碍物的速度 m / 0.1s
    int index_time = 0;                         // 单位 0.1s
    bool isToRight = true;                      // 动态障碍物运动方向，向右
    void global_path_callback(const nav_msgs::Path &msg);

public:
    GenObstacle(/* args */);
    ~GenObstacle();

    void run();
    vector<Location> getObstacle(Location &host);
};

#endif